magnetic.h

Go to the documentation of this file.

// Copyright (c) 2014, 2015, 2016, NXP Semiconductors N.V.,
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of NXP Semiconductors N.V. nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL NXP SEMICONDUCTORS N.V. BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

/*! \file magnetic.h
    \brief Lower level magnetic calibration interface

    Many developers can utilize the NXP Sensor Fusion Library without ever
    making any adjustment to the lower level magnetic calibration functions
    defined in this file.
*/

#ifndef MAGNETIC_H
#define MAGNETIC_H

#ifndef F_USING_MAG
#define F_USING_MAG 0x0002  // normally should be defined in build.h
#endif

#if F_USING_MAG
/// @name Magnetic Calibration Constants
///@{
#define MAGBUFFSIZEX 14             ///< x dimension in magnetometer buffer (12x24 equals 288 elements)
#define MAGBUFFSIZEY (2 * MAGBUFFSIZEX)     ///< y dimension in magnetometer buffer (12x24 equals 288 elements)
#define MINMEASUREMENTS4CAL 110         ///< minimum number of measurements for 4 element calibration
#define MINMEASUREMENTS7CAL 220         ///< minimum number of measurements for 7 element calibration
#define MINMEASUREMENTS10CAL 330        ///< minimum number of measurements for 10 element calibration
#define MAXMEASUREMENTS 360         ///< maximum number of measurements used for calibration
#define CAL_INTERVAL_SECS 300           ///< 300s or 5min interval for regular calibration checks
#define MINBFITUT 10.0F             ///< minimum acceptable geomagnetic field B (uT) for valid calibration
#define MAXBFITUT 90.0F             ///< maximum acceptable geomagnetic field B (uT) for valid calibration
#define FITERRORAGINGSECS 86400.0F      ///< 24 hours: time (s) for fit error to increase (age) by e=2.718
#define MESHDELTACOUNTS 50          ///< magnetic buffer mesh spacing in counts (here 5uT)
#define DEFAULTB 50.0F              ///< default geomagnetic field (uT)
///@}

/// The Magnetometer Measurement Buffer holds a 3-dimensional "constellation"
/// of data points.
///
/// The constellation of points are used to compute magnetic hard/soft iron compensation terms.
/// The contents of this buffer are updated on a continuing basis.
typedef struct MagBuffer
{
    int16_t iBs[3][MAGBUFFSIZEX][MAGBUFFSIZEY];     ///< uncalibrated magnetometer readings
    int32_t index[MAGBUFFSIZEX][MAGBUFFSIZEY];      ///< array of time indices
    int16_t tanarray[MAGBUFFSIZEX - 1];         ///< array of tangents of (100 * angle)
    int16_t iMagBufferCount;                ///< number of magnetometer readings
} MagBuffer;

/// Magnetic Calibration Structure
typedef struct MagCalibration
{
    // start of elements stored to flash memory on Save (16 * 4 = 64 bytes)
    float fV[3];                    ///< current hard iron offset x, y, z, (uT)
    float finvW[3][3];              ///< current inverse soft iron matrix
    float fB;                   ///< current geomagnetic field magnitude (uT)
    float fBSq;                 ///< square of fB (uT^2)
    float fFitErrorpc;              ///< current fit error %
    int32_t iValidMagCal;               ///< solver used: 0 (no calibration) or 4, 7, 10 element
    // end of elements stored to flash memory
    // start of working elements not stored to flash memory
    float ftrV[3];                  ///< trial value of hard iron offset z, y, z (uT)
    float ftrinvW[3][3];                    ///< trial inverse soft iron matrix size
    float ftrB;                 ///< trial value of geomagnetic field magnitude in uT
    float ftrFitErrorpc;                    ///< trial value of fit error %
    float fA[3][3];                 ///< ellipsoid matrix A
    float finvA[3][3];              ///< inverse of ellipsoid matrix A
    float fmatA[10][10];                    ///< scratch 10x10 float matrix used by calibration algorithms
    float fmatB[10][10];                    ///< scratch 10x10 float matrix used by calibration algorithms
    float fvecA[10];                ///< scratch 10x1 vector used by calibration algorithms
    float fvecB[4];                 ///< scratch 4x1 vector used by calibration algorithms
    float fYTY;                 ///< Y^T.Y for 4 element calibration = (iB^2)^2
    int32_t iSumBs[3];              ///< sum of measurements in buffer (counts)
    int32_t iMeanBs[3];             ///< average magnetic measurement (counts)
    int32_t itimeslice;             ///< counter for tine slicing magnetic calibration calculations
    int8_t iCalInProgress;                  ///< flag denoting that a calibration is in progress
    int8_t iNewCalibrationAvailable;            ///< flag denoting that a new calibration has been computed
    int8_t iInitiateMagCal;                 ///< flag to start a new magnetic calibration
    int8_t iMagBufferReadOnly;              ///< flag to denote that the magnetic measurement buffer is temporarily read only
    int8_t i4ElementSolverTried;                ///< flag to denote at least one attempt made with 4 element calibration
    int8_t i7ElementSolverTried;                ///< flag to denote at least one attempt made with 4 element calibration
    int8_t i10ElementSolverTried;               ///< flag to denote at least one attempt made with 4 element calibration
} MagCalibration;


struct MagSensor;  // actual typedef is located in sensor_fusion_types.h

/// @name Function prototypes for functions in magnetic.c
/// These functions comprise the core of the magnetic calibration features of
/// the library.  Parameter descriptions are not included here,
/// as details are provided in sensor_fusion.h.
///@{
void fInitializeMagCalibration(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer);
void iUpdateMagBuffer(struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag, int32_t loopcounter);
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal);
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor* pthisMag, int32_t loopcounter);
void fUpdateMagCalibration4(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag);
void fUpdateMagCalibration7(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag);
void fUpdateMagCalibration10(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag);
void fUpdateMagCalibration4Slice(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag);
void fUpdateMagCalibration7Slice(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag);
void fUpdateMagCalibration10Slice(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag);
///@}
#else    // if F_USING_MAG
typedef struct MagBuffer
{
    void *placeholder;
} MagBuffer;

/// Magnetic Calibration Structure
typedef struct MagCalibration
{
    void *placeholder;
} MagCalibration;
#endif   // if F_USING_MAG
#endif   // #ifndef MAGNETIC_H